#include "VisionToMotors.h"

namespace visiontomotors {
//class VisionToMotors {
//private:
namespace {

int status;
struct addrinfo host_info;       // The struct that getaddrinfo() fills up with data.
struct addrinfo *host_info_list; // Pointer to the to the linked list of host_info's.
int socketfd ; // The socket descripter
int sendData(std::vector<int> data, int length) {
	int msg[length];
	for (int i = 0; i < length; i++) {
		msg[i] = data[i];
	}
	socketfd = socket(host_info_list->ai_family, host_info_list->ai_socktype,
			host_info_list->ai_protocol);
	if (socketfd == -1) {
		std::cout << "socket error " ;
	}

	status = connect(socketfd, host_info_list->ai_addr, host_info_list->ai_addrlen);
	if (status == -1) {
		std::cout << "connect error" ;
	}

	int len;
	ssize_t bytes_sent;
	len = sizeof(msg);
	std::cout << "Len = " << len << std::endl;
	bytes_sent = send(socketfd, &msg, len, 0);
	if (bytes_sent == 0) {
		std::cout << "Failed to send any bytes" << std::endl;
	}

	ssize_t bytes_recieved;
	int incoming_data_buffer[1000];
	bytes_recieved = recv(socketfd, incoming_data_buffer,1000, 0);
	// If no data arrives, the program will just wait here until some data arrives.
	if (bytes_recieved == 0){
		std::cout << "host shut down." << std::endl ;
	}
	if (bytes_recieved == -1) {
		std::cout << "recieve error!" << std::endl ;
	}
	incoming_data_buffer[bytes_recieved] = '\0' ;

	close(socketfd);

	return (int)incoming_data_buffer[0];
}
int write(int word, int id, int address, int value) {
	std::vector<int> msg;
//	msg = {1, word, id, address, value};

	// Code above is the same as below, but slightly faster
	// Code below is provided for easier reading
	msg.resize(5);
		msg[0] = 1;			// We are not using a syncwrite
		msg[1] = word;		// 0 for byte, 1 for word
		msg[2] = id;		// ID to write to
		msg[3] = address;	// Address to write to
		msg[4] = value;		// Value to be written

	int result = sendData(msg, msg.size());

	return result;
}

int read(int word, int id, int address) {
	std::vector<int> msg;
//	msg = {1, word, id, address};

	// Code above is the same as below, but slightly faster
	// Code below is provided for easier reading
		msg.resize(4);
		msg[0] = 1;			// We are not using a syncwrite
		msg[1] = word;		// 0 for byte, 1 for word
		msg[2] = id;		// ID to write to
		msg[3] = address;	// Address to write to

	int result = sendData(msg, msg.size());

	return result;
}


}




bool initClient() {
	bool succeeded = true;
	// The MAN page of getaddrinfo() states "All  the other fields in the structure pointed
	// to by hints must contain either 0 or a null pointer, as appropriate." When a struct
	// is created in c++, it will be given a block of memory. This memory is not nessesary
	// empty. Therefore we use the memset function to make sure all fields are NULL.
	memset(&host_info, 0, sizeof host_info);

	std::cout << "Setting up the structs..."  << std::endl;

	host_info.ai_family = AF_UNSPEC;     // IP version not specified. Can be both.
	host_info.ai_socktype = SOCK_STREAM; // Use SOCK_STREAM for TCP or SOCK_DGRAM for UDP.

	// Now fill up the linked list of host_info structs with google's address information.
	status = getaddrinfo("127.0.0.1", "5557", &host_info, &host_info_list);
	// getaddrinfo returns 0 on succes, or some other value when an error occured.
	// (translated into human readable text by the gai_gai_strerror function).
	if (status != 0)  {
		std::cout << "getaddrinfo error" << gai_strerror(status) ;
		succeeded = false;
	}

	return succeeded;
}

void closeClient() {
	freeaddrinfo(host_info_list);
	close(socketfd);
}

int moveHead(int direction, int speed) {
	std::vector<int> msg(2);
//	msg = {direction, speed};

	msg[0] = direction;
	msg[1] = speed;

	int result = sendData(msg, msg.size());

	return result;
}

double getHeadAngle() {
	int motorpos = read(VISIONTOMOTORS_WORD, 23, PRESENT_POSITION);
	motorpos -= 2048; // Subtract the center position to get the relative location
	double headAngle = motorpos * DEGREES_PER_POSITION * -1;
	return headAngle;
}

double getHeadDownAngle() {
	int motorpos = read(VISIONTOMOTORS_WORD, 24, PRESENT_POSITION);
	int difference = M24_CCW - motorpos;
	double angle = difference * DEGREES_PER_POSITION;
	return angle;
}

bool headLeftRightIsMoving() {
	bool result = false;
	int presentpos = read(VISIONTOMOTORS_WORD, 23, PRESENT_POSITION);
	int goalpos = read(VISIONTOMOTORS_WORD, 23, GOAL_POSITION);
	if (presentpos < (goalpos-25) || presentpos > (goalpos+25)) {
		result = true;
	}
	return result;
}

bool headUpDownIsMoving() {
	bool result = false;
	int presentpos = read(VISIONTOMOTORS_WORD, 24, PRESENT_POSITION);
	int goalpos = read(VISIONTOMOTORS_WORD, 24, GOAL_POSITION);
	if (presentpos < (goalpos-25) || presentpos > (goalpos+25)) {
		result = true;
	}
	return result;
}

void stopHead() {
	write(VISIONTOMOTORS_WORD, 23, MOVING_SPEED, 0);
	write(VISIONTOMOTORS_WORD, 24, MOVING_SPEED, 0);
}

int readMotorPosition(int motor) {
	return (read(VISIONTOMOTORS_WORD, motor, PRESENT_POSITION));
}
//};

} /* namespace visiontomotors */
